//-----------------------------------------------------------------------
//  Form for displaying User Interface
//
//  $File: DriveControl.cs $ $Revision: 1 $
//
//-----------------------------------------------------------------------
using System;
using System.Collections.Generic;
using System.ComponentModel;
//using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;

using System.Drawing.Imaging;
using System.Drawing.Drawing2D;

// Added references
// Because this is a standard WinForm, it does not automatically get CCR/DSS
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.ServiceModel.Dssp;

namespace ProMRDS.Robotics.DriveByWire
{
    public partial class DriveControl : Form
    {
        // This port is passed across to the constructor
        // It allows messages to be sent back to the main service
        DriveControlEvents _eventsPort;
        // This is part of the main service State
        public GUIOptions options;

        #region Initialization and Shutdown

        public DriveControl(DriveControlEvents EventsPort, DriveByWireState state)
        {
            InitializeComponent();

            // Remember the port to use to send back messages
            _eventsPort = EventsPort;

            // Copy the option settings
            options = state.Options;

#if !NET_CF20
            txtPort.ValidatingType = typeof(ushort);

            this.StartPosition = FormStartPosition.Manual;
            this.Location = new Point(options.WindowStartX, options.WindowStartY);
#endif

            // TT - Add initialization code based on the saved State
            // Set up the connection parameters
            if (state.Host != "")
                txtHost.Text = state.Host;
            if (state.Port != 0)
                txtPort.Text = state.Port.ToString();

            this.chkFixedMoves.Checked = options.FixedMoves;

            // The dead zone can't be negative, but it can be zero
            options.DeadZoneX = Math.Abs(state.Options.DeadZoneX);
            options.DeadZoneY = Math.Abs(state.Options.DeadZoneY);

            // Just in case the scale factors have not been initialized
            if (state.Options.TranslateScaleFactor == 0)
                options.TranslateScaleFactor = 1.0;
            else
                options.TranslateScaleFactor = state.Options.TranslateScaleFactor;
            if (state.Options.RotateScaleFactor == 0)
                options.RotateScaleFactor = 0.5;
            else
                options.RotateScaleFactor = state.Options.RotateScaleFactor;
        }

        private void DriveControl_Load(object sender, EventArgs e)
        {
            _eventsPort.Post(new OnLoad(this));
            ProcessTrackball(0, 0);
        }

        private void DriveControl_Closed(object sender, EventArgs e)
        {
            _eventsPort.Post(new OnClosed(this));
        }

        #endregion

        #region Event Handlers

        private void btnConnect_Click(object sender, EventArgs e)
        {
            string host = txtHost.Text;

            if (host.Length == 0)
            {
                txtHost.Focus();
                return;
            }

            // Can't do this on CF
            //object obj = txtPort.ValidateText();
            object obj = null;
            try
            {
                obj = int.Parse(txtPort.Text);
            }
            catch (Exception ex)
            {
            }
            if (obj == null)
                obj = (int)0;
            int port = (int)obj;
            if (port == 0)
            {
                txtPort.Focus();
                return;
            }

            UriBuilder builder = new UriBuilder(Schemes.DsspTcp, host, port, ServicePaths.InstanceDirectory);

            _eventsPort.Post(new OnConnect(this, builder.ToString()));

        }

        private void btnExit_Click(object sender, EventArgs e)
        {
            this.Close();
        }

        #endregion

        public void ProcessTrackball(int x, int y)
        {
            lblX.Text = x.ToString();
            lblY.Text = y.ToString();
            //lblZ.Text = axes.Z.ToString();

            DrawJoystick(x, y);

            double left;
            double right;

            //this is the raw length of the vector
            double magnitude = Math.Sqrt(x * x + y * y);

            // Check for the "dead zone"
            // Added some code so that the speed values would
            // not suddenly jump after leaving the Dead Zone
            if (Math.Abs(x) < options.DeadZoneX)
                x = 0;
            else
            {
                // Subtract off the dead zone value so that the
                // coord starts from zero
                if (x > 0)
                    x -= (int)options.DeadZoneX;
                else
                    x += (int)options.DeadZoneX;
            }
            if (Math.Abs(y) < options.DeadZoneY)
                y = 0;
            else
            {
                if (y > 0)
                    y -= (int)options.DeadZoneY;
                else
                    y += (int)options.DeadZoneY;
            }

            if (x == 0 && y == 0)
            {
                // Totally dead in the middle!
                left = right = 0;
            }
            else
            {
                //angle of the vector
                double theta = Math.Atan2(y, x);

                //this is the maximum magnitude for a given angle
                //                    double maxMag;
                double scaledMag = 1.0;
                //a scaled down magnitude according to above
                //                    double scaledMag = magnitude * 1000 / maxMag;
                scaledMag = magnitude;
                //decompose the vector into motor components
                // What is the significance of 150? The cross-hairs
                // cross over at 0, so this just meant that the
                // cross-over point was slightly below the center
                // of the yoke ...
                // NOTE: There is a peculiar problem that if you
                // try to rotate the robot on the spot, and you
                // are not careful to keep the cursor on the
                // horizontal center line, then the robot might
                // suddenly start rotating in the OPPOSITE direction!
                // This is a quirk of the maths. It happens if the
                // cursor moves outside the dead zone on the bottom
                // (negative) side. This might be the reason for the
                // -150 in the original code, i.e. to try to avoid
                // this problem.
                //if (data.Y > -150)
                if (y >= 0)
                {
                    left = scaledMag * options.TranslateScaleFactor * Math.Sin(theta) + scaledMag * options.RotateScaleFactor * Math.Cos(theta);
                    right = scaledMag * options.TranslateScaleFactor * Math.Sin(theta) - scaledMag * options.RotateScaleFactor * Math.Cos(theta);
                }
                else
                {
                    left = scaledMag * options.TranslateScaleFactor * Math.Sin(theta) - scaledMag * options.RotateScaleFactor * Math.Cos(theta);
                    right = scaledMag * options.TranslateScaleFactor * Math.Sin(theta) + scaledMag * options.RotateScaleFactor * Math.Cos(theta);
                }
            }
            //cap at 1000
            left = Math.Min(left, 1000);
            right = Math.Min(right, 1000);
            left = Math.Max(left, -1000);
            right = Math.Max(right, -1000);
            // Quick and dirty way to display results for debugging -
            // Uncomment the two lines below
            //                Console.WriteLine("Joy: " + data.X + ", " + data.Y
            //                            + " => " + left + ", " + right);
            _eventsPort.Post(new OnMove(this, (int)Math.Round(left), (int)Math.Round(right)));
        }

        private void DrawJoystick(int x, int y)
        {
            Bitmap bmp = new Bitmap(picTrackBall.Width, picTrackBall.Height);
            Graphics g = Graphics.FromImage(bmp);

            int width = bmp.Width - 1;
            int height = bmp.Height - 1;

            /*
            // Most of these properties and methods are not supported on CF!
            g.Clear(Color.Transparent);
            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;
            GraphicsPath path = new GraphicsPath();
            path.AddEllipse(0, 0, width, height);

            PathGradientBrush pathBrush = new PathGradientBrush(path);
            pathBrush.CenterPoint = new PointF(width / 3f, height / 3f);
            pathBrush.CenterColor = Color.White;
            pathBrush.SurroundColors = new Color[] { Color.LightGray };

            g.FillPath(pathBrush, path);
            g.DrawPath(Pens.Black, path);

            int partial = y * height / 2200;
            if (partial > 0)
            {
                g.DrawArc(Pens.Black,
                    0,
                    height / 2 - partial,
                    width,
                    2 * partial,
                    180,
                    180);
            }
            else if (partial == 0)
            {
                g.DrawLine(Pens.Black, 0, height / 2, width, height / 2);
            }
            else
            {
                g.DrawArc(Pens.Black,
                    0,
                    height / 2 + partial,
                    width,
                    -2 * partial,
                    0,
                    180);
            }

            partial = x * width / 2200;
            if (partial > 0)
            {
                g.DrawArc(Pens.Black,
                    width / 2 - partial,
                    0,
                    2 * partial,
                    height,
                    270,
                    180);
            }
            else if (partial == 0)
            {
                g.DrawLine(Pens.Black, width / 2, 0, width / 2, height);
            }
            else
            {
                g.DrawArc(Pens.Black,
                    width / 2 + partial,
                    0,
                    -2 * partial,
                    height,
                    90,
                    180);
            }
             */

            int picX, picY;
            Pen pBlack = new Pen(Color.Black);
            Pen pRed = new Pen(Color.Red);
            SolidBrush bWhite = new SolidBrush(Color.White);

            picX = (x + 1000) * width / 2000;
            picY = (-y + 1000) * height / 2000;

            // Create a white circle with black border
            // This is a fair amount of overhead to make it look pretty,
            // and besides the coords need to be scaled to fit the circle
            //g.FillEllipse(bWhite, 0, 0, width, height);
            //g.DrawEllipse(pBlack, 0, 0, width, height);


            // Draw a black box as a frame around a white rectangle
            g.FillRectangle(bWhite, 0, 0, width, height);
            g.DrawRectangle(pBlack, 0, 0, width, height);

            // Draw four lines
            g.DrawLine(pRed, 0, height / 2, picX, picY);
            g.DrawLine(pRed, picX, picY, width, height / 2);
            g.DrawLine(pRed, width / 2, 0, picX, picY);
            g.DrawLine(pRed, picX, picY, width / 2, height);

            picTrackBall.Image = bmp;
        }


        private void picTrackBall_MouseMove(object sender, MouseEventArgs e)
        {
            // Mouse movements use the Left button only, i.e. drag
            if (e.Button == MouseButtons.Left)
            {
                int x, y;

                // Get cursor coords
                x = Math.Min(picTrackBall.Width, Math.Max(e.X, 0));
                y = Math.Min(picTrackBall.Height, Math.Max(e.Y, 0));

                // Translate to a range of -1000 to +1000
                x = x * 2000 / picTrackBall.Width - 1000;
                y = y * 2000 / picTrackBall.Height - 1000;

                // Fugde a Game Controller update and "send" it
                ProcessTrackball(x, -y);
            }

        }

        private void picTrackBall_MouseUp(object sender, MouseEventArgs e)
        {
            // Center the trackball and stop moving
            ProcessTrackball(0, 0);
        }

        #region Motion Control Buttons

        // These buttons use RotateDegrees and DriveDistance which are not implemented
        // by all robots.

        private void btnLeft_Click(object sender, EventArgs e)
        {
            if (chkFixedMoves.Checked)
                _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Rotate, options.RotateAngle, options.MotionSpeed));
            else
                TurnLeft();
        }

        private void btnRight_Click(object sender, EventArgs e)
        {
            if (chkFixedMoves.Checked)
                _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Rotate, -options.RotateAngle, options.MotionSpeed));
            else
                TurnRight();
        }

        private void btnForward_Click(object sender, EventArgs e)
        {
            if (chkFixedMoves.Checked)
                _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Translate, options.DriveDistance / 1000, options.MotionSpeed));
            else
                Forward();
        }

        private void btnReverse_Click(object sender, EventArgs e)
        {
            if (chkFixedMoves.Checked)
                _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Translate, options.DriveDistance / 1000, -options.MotionSpeed));
            else
                Backward();
        }

        private void btnStop_Click(object sender, EventArgs e)
        {
            _eventsPort.Post(new OnMotionCommand(this, MOTION_COMMANDS.Stop, 0, 0));
        }

        #endregion

        private void chkFixedMoves_CheckStateChanged(object sender, EventArgs e)
        {

        }

        #region Keyboard Handlers

        // NOTE: The arrow keys will not normally appear in a KeyDown event.
        // This is because they will be pre-processed. However, the operation
        // under CF is different and it does no harm to leave them in here.

        private void DriveControl_KeyDown(object sender, KeyEventArgs e)
        {
            switch ((Keys)e.KeyValue)
            {
                case Keys.Up:
                    Forward();
                    e.Handled = true;
                    break;

                case Keys.Down:
                    Backward();
                    e.Handled = true;
                    break;

                case Keys.Left:
                    TurnLeft();
                    e.Handled = true;
                    break;

                case Keys.Right:
                    TurnRight();
                    e.Handled = true;
                    break;

                default:
                    break;
            }
        }

        private void DriveControl_KeyUp(object sender, KeyEventArgs e)
        {
            Stop();
        }
        void Forward()
        {
            _eventsPort.Post(new OnMove(this, (int)options.MotionSpeed, (int)options.MotionSpeed));
        }

        void Backward()
        {
            _eventsPort.Post(new OnMove(this, (int)(-options.MotionSpeed), (int)(-options.MotionSpeed)));
        }

        void TurnLeft()
        {
            _eventsPort.Post(new OnMove(this, (int)(-options.MotionSpeed), (int)options.MotionSpeed));
        }

        void TurnRight()
        {
            _eventsPort.Post(new OnMove(this, (int)options.MotionSpeed, (int)(-options.MotionSpeed)));
        }

        void Stop()
        {
            _eventsPort.Post(new OnMove(this, 0, 0));
        }

        #endregion

    }
}